clear; close all; clc;

saveArg = 0;

xCorrect = 23;
yCorrect = 86.542;
medSize = 110;
labelSzAx = 16;
Lb = 0.194;
stdThreshold = 0.2;
mark = round(50/70*medSize,0);
listFile = dir('..\Exp_Prev_C1\Data_*');
indNf = 26;
indFd = 27;


%% Reading data

x = nan(medSize,medSize,length(listFile));
y = x;
u = x;
v = x;
h = waitbar(0,'Processing...');
for i = 1:length(listFile)
    dataC = importdata(sprintf('..\\Exp_Prev_C1\\%s\\B00001.dat',...
        listFile(i).name));
    dataC = dataC.data;
    xc = (dataC(:,1) - xCorrect)/1000;
    yc = (dataC(:,2) - yCorrect)/1000;
    uc = dataC(:,3);
    vc = dataC(:,4);
    noInd = find(uc==0&vc==0);
    xc(noInd) = [];
    yc(noInd) = [];
    uc(noInd) = [];
    vc(noInd) = [];
    
    xlin = linspace(min(xc),max(xc),medSize);
    ylin = linspace(min(yc),max(yc),medSize);
    for j1 = 2:medSize-1
        for j2 = 2:medSize-1
            indCheck = xc>xlin(j2-1) & xc<xlin(j2+1) & ...
                yc>ylin(j1-1) & yc<ylin(j1+1);
            xCheck = xc(indCheck);
            yCheck = yc(indCheck);
            uCheck = uc(indCheck);
            vCheck = vc(indCheck);
            x(j1,j2,i) = median(xCheck(isnan(xCheck)==0));
            y(j1,j2,i) = median(yCheck(isnan(xCheck)==0));
            u(j1,j2,i) = median(uCheck(isnan(xCheck)==0));
            v(j1,j2,i) = median(vCheck(isnan(xCheck)==0));
        end
    end
    clear *Check *c;
    
    vectorSlot = sqrt(u(:,:,i).^2+v(:,:,i).^2);
    thetaSlot = acos(u(:,:,i)./vectorSlot);
    
    theta_STD = nan(medSize,medSize);
    for j1 = 2:medSize-1
        for j2 = 2:medSize-1
            thetaCheck = thetaSlot(j1-1:j1+1,j2-1:j2+1);
            theta_STD(j1,j2) = std(thetaCheck(isnan(thetaCheck)==0));
        end
    end
    uSlot = u(:,:,i);
    vSlot = v(:,:,i);
    uSlot(theta_STD>stdThreshold) = NaN;
    vSlot(theta_STD>stdThreshold) = NaN;
    u(:,:,i) = uSlot;
    v(:,:,i) = vSlot;
    
    waitbar(i/length(listFile),h);
end
close(h);
clear *Slot *Check dataC;

uBounds = nan(medSize,medSize,3);
vBounds = uBounds;
for j1 = 2:medSize-1
    for j2 = 2:medSize-1
        ind1Check = find(isnan(u(j1,j2,:))==0,1,'first');
        ind2Check = indNf;
        ind3Check = indFd;
        if isempty(ind1Check)==0 && isempty(ind2Check)==0 && ...
                ind1Check ~= ind2Check
            uBounds(j1,j2,1) = u(j1,j2,ind1Check);
            vBounds(j1,j2,1) = v(j1,j2,ind1Check);
            uBounds(j1,j2,2) = u(j1,j2,ind2Check);
            vBounds(j1,j2,2) = v(j1,j2,ind2Check);
            uBounds(j1,j2,3) = u(j1,j2,ind3Check);
            vBounds(j1,j2,3) = v(j1,j2,ind3Check);
        end
    end
end
clear *Check *STD *lin noInd h;

x = x(:,:,indFd);
y = y(:,:,indFd);
u = uBounds;
v = vBounds;
clear *Bounds;


%% Post-proc

vector = sqrt(u.^2+v.^2);
% theta = acos(u./vector);
% theta(theta<pi/2) = pi - theta(theta<pi/2);

theta = nan(size(vector));
theta(v>0) = atan(u(v>0)./v(v>0));
theta(u>0&v<0) = pi + atan(u(u>0&v<0)./v(u>0&v<0));
theta(u<0&v<0) = -pi + atan(u(u<0&v<0)./v(u<0&v<0));

slice = theta(:,:,2);
slice(slice<-pi/2|slice>pi/2) = NaN;
theta(:,:,2) = slice;
slice = theta(:,:,3);
slice(slice<-pi/2|slice>pi/2) = NaN;
theta(:,:,3) = slice;

dtheta = -(theta(:,:,2:3) - theta(:,:,1));
dtheta = abs(dtheta);
% dtheta(dtheta>pi/2) = pi - dtheta(dtheta>pi/2);

xs = reshape(x,numel(x),1);
ys = reshape(y,numel(y),1);
ts1 = reshape(dtheta(:,:,1),numel(dtheta(:,:,1)),1);
ts2 = reshape(dtheta(:,:,2),numel(dtheta(:,:,2)),1);
ts3 = reshape(theta(:,:,1),numel(theta(:,:,1)),1);
ts4 = reshape(theta(:,:,2),numel(theta(:,:,2)),1);
ts5 = reshape(theta(:,:,3),numel(theta(:,:,3)),1);

ts1(ts1>=pi) = NaN;
ts2(ts2>=pi) = NaN;

xlin = linspace(min(min(x)),max(max(x)),30);
tM = nan(length(xlin),4);
for j2 = 1:length(xlin)-1
    indCheck = ts1(x>=xlin(j2)&x<=xlin(j2+1));
    tM(j2,1) = median(indCheck(isnan(indCheck)==0));
    tM(j2,2) = mad(indCheck(isnan(indCheck)==0));
    indCheck = ts2(x>=xlin(j2)&x<=xlin(j2+1));
    tM(j2,3) = median(indCheck(isnan(indCheck)==0));
    tM(j2,4) = mad(indCheck(isnan(indCheck)==0));
end
clear indCheck;


%% Plot 1 Theta

xlimits = [-0.1,0.1];
ylimits = [-0.37,0];
quivSkip = 6;
quivSize = 1;

figure(1); clf;
set(gcf,'units','normalized','outerposition',[0.1 0.1 0.8 0.8]);
subplot(1,2,1);
    scatter(xs,ys,30,ts1,'filled'); hold on;
    quiver(x(1:quivSkip:end,1:quivSkip:end),...
        y(1:quivSkip:end,1:quivSkip:end),...
        u(1:quivSkip:end,1:quivSkip:end,1)./...
        vector(1:quivSkip:end,1:quivSkip:end,1),...
        v(1:quivSkip:end,1:quivSkip:end,1)./...
        vector(1:quivSkip:end,1:quivSkip:end,1),...
        quivSize,'w');
    quiver(x(1:quivSkip:end,1:quivSkip:end),...
        y(1:quivSkip:end,1:quivSkip:end),...
        u(1:quivSkip:end,1:quivSkip:end,2)./...
        vector(1:quivSkip:end,1:quivSkip:end,2),...
        v(1:quivSkip:end,1:quivSkip:end,2)./...
        vector(1:quivSkip:end,1:quivSkip:end,2),...
        quivSize,'y');
    ylabel('y (mm)');
    title('Non-feeder','FontWeight','normal');
subplot(1,2,2);
    scatter(xs,ys,30,ts2,'filled'); hold on;
    quiver(x(1:quivSkip:end,1:quivSkip:end),...
        y(1:quivSkip:end,1:quivSkip:end),...
        u(1:quivSkip:end,1:quivSkip:end,1)./...
        vector(1:quivSkip:end,1:quivSkip:end,1),...
        v(1:quivSkip:end,1:quivSkip:end,1)./...
        vector(1:quivSkip:end,1:quivSkip:end,1),...
        quivSize,'w');
    quiver(x(1:quivSkip:end,1:quivSkip:end),...
        y(1:quivSkip:end,1:quivSkip:end),...
        u(1:quivSkip:end,1:quivSkip:end,3)./...
        vector(1:quivSkip:end,1:quivSkip:end,3),...
        v(1:quivSkip:end,1:quivSkip:end,3)./...
        vector(1:quivSkip:end,1:quivSkip:end,3),...
        quivSize,'y');
    title('Feeder','FontWeight','normal');

for i = 1:2
    subplot(1,2,i);
    caxis([0,pi]);
    xlim(xlimits); ylim(ylimits);
    xlabel('x (mm)');
    set(gca,'FontSize',14);
    c=colorbar;
    set(c,'Ticks',[0,pi],'TickLabels',[]);
end
set(c,'Ticks',[0,pi],'TickLabels',[0,180]);
ylabel(c,'\theta_f-\theta_0 (deg)');


%% Plot 2 Vs x loc

figure(2); clf;
errorbar(xlin/Lb,tM(:,1)*180/pi,tM(:,2)*180/pi,'b.','markersize',16); hold on;
errorbar(xlin/Lb,tM(:,3)*180/pi,tM(:,4)*180/pi,'r.','markersize',16);

xlabel('x (m)'); ylabel('\Delta\theta (deg)');
set(gca,'FontSize',14);
legend('Non-feeder','Feeder','location','northeast');


%% Saving

if saveArg == 1
    save('results.mat');
    
    figure(1);
    outFile = 'dtheta';
    print(gcf,outFile,'-djpeg','-r300');
    
    figure(2);
    outFile = 'vs_x';
    print(gcf,outFile,'-djpeg','-r300');
end



